#!/usr/bin/env python
#coding:utf-8

import socket
import struct
import Queue
import time
import threading
from Debug import debug, error

class Foc:
    def __init__(self, name, ip, port):
        self.name = name
        self.ip = ip
        self.port = port
        self.socket = None
        self.Listener = None
        self.alive = False
        self.thread = None
        self.athrad = None
        
    def _recv_(self):
        while self.alive:
            try:
                buf = self.Recv()
                if buf:
                    res = self.AcceptResult(buf)
                    if res:
                        self.Listener(res)
            except:
                pass
            time.sleep(0.1)
            
    def _alive_(self):
        while self.alive:
            try:
                self.socket.send('00209999            \x00')
                #time.sleep(1)
                #buf = self.socket.recv(1024)
                #print buf
            except:
                error.error(u"FOC:%s 发送Alive命令失败" % self.name)
                time.sleep(0.5)
                try:
                    self.socket.send('00209999            \x00')
                except:
                    self.Reconnect()
            time.sleep(7)
            
    def Recv(self):
        h = self.socket.recv(20)
        if h:
            len = int(h[0:4]) - 20 + 1
            d = self.socket.recv(len)
            return ''.join([h, d])
        
    def ResultSubscribe(self):
        self.socket.send('002000600011        \x00')
        buf = self.Recv()
        print buf
        if not buf:
            error.error(u"FOC:%s 控制器没有响应请求" % self.name)
            return
            
        if buf[4:8] != '0005':
            error.error(u"FOC:%s 命令错误" % self.name)
            
    def ResultUnsubscribe(self):
        self.socket.send('00200063            \x00')
        buf = self.Recv()
        if not buf:
            error.error(u"FOC:%s 控制器没有响应请求" % self.name)
            return
            
        if buf[4:8] != '0005':
            error.error(u"FOC:%s 命令错误" % self.name)
            
    def AcceptResult(self, buf):
        if buf[4:8] == '0061':
            if buf[11] == '0':
                self.socket.send('00200062            \x00')
                
            try:
                t = float(buf[140:146])/100.0  #torque has been multiplied 100
            except:                            #when accepted.
                t = 0.0
            try:
                a = float(buf[169:174])  #angle
            except:
                a = 0.0
            return t, a, buf[107] == '1' and 'OK' or 'NOK'
        return False
        
    def Connect(self, timeout = 2):
        try:
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.socket.settimeout(timeout)
            self.socket.connect((self.ip, self.port))
            self.alive = True
            
            self.socket.send('00200001003         \x00')
            buf = self.Recv()
            if not buf:
                self.socket.close()
                error.error(u"FOC:%s 控制器没有响 应，连接失败" % self.name)
                
            mid = buf[4:8]
            if mid == '0002':
                self.ResultSubscribe()
                self.thread = threading.Thread(target = self._recv_)
                self.thread.start()
                self.athrad = threading.Thread(target = self._alive_)
                self.athrad.start()
                
            else:
                error.error(u'FOC:%s 响应命令错误' % self.name)
        except:
            error.traceback()
            self.socket = None
            raise
    
    def Disconnect(self):
        if not self.thread:
            return
        
        self.alive = False
        self.athrad.join()
        self.athrad = None
        self.thread.join()
        self.thread = None
        
        try:
            self.ResultUnsubscribe()
            
            self.socket.send("00200003            \x00")
            buf = self.Recv()
            if not buf:
                error.error(u'FOC:%s 命令没响应' % self.name)
            self.socket.close()
        except:
            error.traceback()
            raise
        
    def Reconnect(self):
        self.Disconnect()
        self.Connect()
    
class Mac(Foc):
    def __init__(self, name, ip, port):
        Foc.__init__(self, name, ip, port)
        
    def ResultSubscribe(self):
        self.socket.send('00200105            \x00')
        buf = self.Recv()
        if not buf:
            error.error(u"FOC:%s 控制器没有响应请求" % self.name)
            return
            
        if buf[4:8] != '0005':
            error.error(u"FOC:%s 命令错误" % self.name)
            
    def ResultUnsubscribe(self):
        self.socket.send('00200109            \x00')
        buf = self.Recv()
        if not buf:
            error.error(u"FOC:%s 控制器没有响应请求" % self.name)
            return
            
        if buf[4:8] != '0005':
            error.error(u"FOC:%s 命令错误" % self.name)
            
    def AcceptResult(self, buf):
        if buf[4:8] == '0106':
            self.socket.send('00210108            0\x00')
            res = []
            cnt = int(buf[163:165])
            for i in range(cnt):
                sp = 165 + i * 67
                try:
                    t = float(buf[sp + 15:sp + 22])
                except:
                    t = 0.0
                try:
                    a = float(buf[sp + 24:sp + 31])
                except:
                    a = 0.0
                res.append((t, a, buf[sp + 6] == '1' and 'OK' or 'NOK'))
            return tuple(res)
        return False

if __name__ == '__main__':
    f = Foc('F', '192.168.1.19', 4545)
    f.Connect()
    '''
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    
    s.connect(('192.168.1.19',4545))
    
    s.send('00200001003         \x00') #request to connect
    print s.recv(1024)
    s.send('00209999            \x00')
    print s.recv(1024)
    print '----------------------------------'
    time.sleep(1)
    s.send('00200105            \x00') #get macs data
    #s.send('002000600011        \x00')
    print s.recv(1024)
    time.sleep(1)
    s.setblocking(True)
    print s.recv(1024)
    print "end"
    '''
